function [xtt, Sigtt, xttminus1, Sigttminus1,L] = KalmanFilter(y,z,a,x0,Sig0,D,F,H,A,Q,R,G)
%KalmanFilter.m This function does the time-invariant
%   Kalman Filter recursion
%   The State Space system is written
%   x(t) = D * a(t) + F * x(t-1) + G * epsilon(t)
%   y(t) = A * z(t) + H * x(t) + u(t)
%
%   where var(epsilon) = Q;
%   and var(u) = R;
%
%   a(t) and z(t) is a vector of exogenous variables (for example constants or
%   dummies) that affect the state and observation equation respectively
%   x0 and Sig0 are the initial conditions for the state vector mean and variance
% tic

k = size(F,1);
n = size(y,2);
T = size(y,1);

if isempty(G), G=eye(k); end
if isempty(D), D = zeros(k,1); end
if isempty(a), a = zeros(T,1); end
if isempty(A), A = zeros(n,1); end
if isempty(z), z = ones(T,1); end


% Allocating memory

Sigtt = nan(k,k,T);
Sigttminus1 = nan(k,k,T);

xtt = nan(k,T);
xttminus1 = nan(k,T);

like = zeros(T,1);


for t=1:T
    
    if ~ismatrix(H)
        Ht = H(:,:,t);
    else Ht = H;
    end
    
    if ~ismatrix(F)
        Ft = F(:,:,t);
    else Ft = F;
    end
    
    if ~ismatrix(Q)
        Qt = Q(:,:,t);
    else Qt = Q;
    end
    
    if ~ismatrix(R)
        Rt = R(:,:,t);
    else Rt = R;
    end
    
    at = a(t,:)';
    zt = z(t,:)';
    yt = y(t,:)';
     
    % The following lines adjust the system for any missing values in
    % the data
    
    H1=Ht;
    A1=A;
    R1=Rt;
    
    index = find(isnan(yt));
    
    H1(index,:) = 0;
    A1(index,:) = 0;   
    
    for j = 1:size(index,1)
        jj = index(j,1);
        R1(jj,jj) = 1;
    end
    
    yt(index) = 0;
    
    % Start of Regular Kalman Filter recursion
    
    % For Faster Implementation

%     Ft = sparse(Ft);
%     Qt = sparse(Qt);
%     H1 = sparse(H1);
    
    % Prediction
    
    if t == 1                                          % Initialisation
        xttminus1(:,t) = D*at + Ft * x0;
        Sigttminus1(:,:,t) = Ft*Sig0*Ft' + G*Qt*G';
    else                                            % Predict the State
        xttminus1(:,t) = D*at + Ft * xtt(:,t-1);
        Sigttminus1(:,:,t) = Ft * Sigtt(:,:,t-1) * Ft' + G*Qt*G';
    end
    
    yhat  = yt - A1*zt - H1*xttminus1(:,t);      % Predict the Observables
    fhat = H1 * Sigttminus1(:,:,t) * H1' + R1;
    
    % Updating
    
    % Standard form
    
    %
    
%     if isdiag(fhat)
%         inv_fhat = diag(1./diag(fhat));
%         K = Sigttminus1(:,:,t) * H1'*inv_fhat;
%     else
% %         K = Sigttminus1(:,:,t) * H1'*inv(fhat);          % Kalman Gain
%         K = Sigttminus1(:,:,t) * (H1'/fhat);          % Kalman Gain 
%     end
%     
% 
%     
%     xtt(:,t) = xttminus1(:,t) + K * yhat;          % Update State
%     
%     Sigtt(:,:,t) = (eye(k) - K * H1) * Sigttminus1(:,:,t) * ...
%         (eye(k) - K * H1)' + K*R1*K';
    
    % Last equation is not the standard way
    % of writing this, avoids substraction
    % of two positive definite matrices
    % which can lead to rounding errors
    
    % Square Root Form
    
                Zf = chol(Sigttminus1(:,:,t),'lower');
                [C,Tau,~] = svd(Zf'*H1'*diag(1./diag(R1))*H1*Zf);
                Za = Zf*C*diag((1+diag(Tau)).^(-0.5));
                Sigtt_temp = Za*Za';
    
                K  = Sigtt_temp * H1' * diag(1./diag(R1));
    
                xtt(:,t) = xttminus1(:,t) + K * yhat;
                Sigtt(:,:,t) = Sigtt_temp;
    
    % Compute the likelihood
    
                like(t)  = -0.5*log(det(fhat)) - 0.5*yhat'*inv2(fhat)*yhat; % Check
    
end

xtt = xtt';
xttminus1 = xttminus1';

L = -(sum(like(1:end)) - (T)/2*log(2*pi));
% toc
end


